#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <image_transport/image_transport.hpp>

class CameraPublisher : public rclcpp::Node
{
public:
    CameraPublisher() : Node("camera_publisher")
    {
        // 创建图像发布者
        image_pub_ = this->create_publisher<sensor_msgs::msg::Image>("camera_2", 10);

        // 打开指定的摄像头设备
        cap_.open("/dev/video0"); // 指定设备路径
        if (!cap_.isOpened())
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to open camera at /dev/video1!");
            return;
        }

        // 设置定时器，每30ms发布一次图像
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(30),
            std::bind(&CameraPublisher::timer_callback, this));
    }

private:
    void timer_callback()
    {
        cv::Mat frame;
        if (cap_.read(frame))
        {
            // 创建消息头并设置时间戳
            auto header = std_msgs::msg::Header();
            header.stamp = this->now();
            header.frame_id = "camera_frame"; // 可自定义帧ID

            // 将OpenCV图像转换为ROS消息
            auto msg = sensor_msgs::msg::Image();
            cv_bridge::CvImage cv_image(header, "bgr8", frame);
            cv_image.toImageMsg(msg);
            image_pub_->publish(msg);
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to read frame from camera!");
        }
    }
    rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
    cv::VideoCapture cap_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<CameraPublisher>());
    rclcpp::shutdown();
    return 0;
}